 #include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
int servoValuesInit[4] = {290, 305, 305, 285};

int cmdNum=0;
int cmdValue=0;

int lightPin[3] = {22, 23, 24};
int lightStatus[3] = {LOW, LOW, LOW};
long lightTimeStamp[3] = {0, 0, 0};

//Mather Power
int powerMotorEn[6] = {4,5,6,7,8,9};
int powerMotorInA[12] = {42,44,46,48,50,52};
int powerMotorInB[12] = {43,45,47,49,51,53};

//Sun Power
int sunSwitchPin[2] = {2,3};

int sunMotorInA[2] = {38,40};
int sunMotorInB[2] = {39,41};
boolean isSunDeviceRuning = false;
long runSunDeviceTimeStamp = 0;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  
  pwm.begin();
  pwm.setPWMFreq(50);  // Analog servos run at ~60 Hz updates

  while (!Serial) {
  ; // wait for serial port to connect. Needed for Leonardo only
  }
  
  initServo();  
  
  for (int i=0;i<3;i++) {
    pinMode(lightPin[i], OUTPUT);
    digitalWrite(lightPin[i], lightStatus[i]);
  }
  
  for (int i=0;i<6;i++) {
    pinMode(powerMotorEn[i], OUTPUT);
    pinMode(powerMotorInA[i], OUTPUT);
    pinMode(powerMotorInB[i], OUTPUT);
  }
  
  for (int i=0;i<2;i++) {
    pinMode(sunSwitchPin[i], INPUT_PULLUP);
    pinMode(sunMotorInA[i], OUTPUT);
    pinMode(sunMotorInB[i], OUTPUT);
  }
  
  if (  (digitalRead(sunSwitchPin[0]) == HIGH) || (digitalRead(sunSwitchPin[1]) == HIGH)  )
    openSunDevice();
    
    //openSunDevice();
    /*
      while ( 1 ) {
      digitalWrite(sunMotorInA[0], HIGH);
      digitalWrite(sunMotorInB[0], LOW);
  }
  */
  
}

void loop() {
  
  if (receiveCMD()) {
    Serial.print(cmdNum);
    Serial.print(" ");
    Serial.println(cmdValue);
    
    if ( (cmdNum == 31) || (cmdNum == 32) || (cmdNum == 30)) {
      runPowerMotor(cmdNum, cmdValue);
      //Power Go!
    } else if ( (cmdNum == 70) && (!isSunDeviceRuning) ) {
      //Sun Device
      runSunDevice(cmdValue);
    } else if ( cmdNum == 80 ) {
      //  Light
      runLight(cmdValue);
    } else if ( cmdNum == 0 ) {
      //Servo
      int d = 300 - cmdValue;
      pwm.setPWM(0, 0, servoValuesInit[0] + d );
      pwm.setPWM(1, 0, servoValuesInit[1] + d );
      pwm.setPWM(2, 0, servoValuesInit[2] - d );
      pwm.setPWM(3, 0, servoValuesInit[3] - d );
    }
  }
  delay(10);
  
}

void runSunDevice(int cmdValue) {
  if ( cmdValue == 0 ) {
    isSunDeviceRuning = true;
    closeSunDevice();
  } else if (cmdValue== 1) {
    isSunDeviceRuning = true;
    openSunDevice();
  }
}

void openSunDevice() {
  if (  (digitalRead(sunSwitchPin[0]) == HIGH) || (digitalRead(sunSwitchPin[1]) == HIGH)  )
    closeSunDevice();
    
   runOpenSunDevice(1, 13000);
   runOpenSunDevice(0, 12000);
   isSunDeviceRuning = false;
}

void runOpenSunDevice(int num, int time) {
  digitalWrite(sunMotorInA[num], HIGH);
  digitalWrite(sunMotorInB[num], LOW);
  delay( time );
  digitalWrite(sunMotorInA[num], HIGH);
  digitalWrite(sunMotorInB[num], HIGH);
}

void closeSunDevice() {
  if ( (digitalRead(sunSwitchPin[0]) == LOW) && (digitalRead(sunSwitchPin[1]) == LOW) ) {
    //do nothing
    Serial.println("LOW LOW!");
  } else if ( (digitalRead(sunSwitchPin[0]) == LOW) && (digitalRead(sunSwitchPin[1]) == HIGH) )  {
    // close Right
    Serial.println("LOW HIGH!");
    runCloseSunDevice(1);
  } else if ( (digitalRead(sunSwitchPin[0]) == HIGH) && (digitalRead(sunSwitchPin[1]) == LOW) ) {
    //do nothing
    Serial.println("HIGH LOW!");
  } else {
    //close Left
    Serial.println("HIGH HIGH!");
    runCloseSunDevice(0);
    //close Right
    runCloseSunDevice(1);
  }
  isSunDeviceRuning = false;
}

void runCloseSunDevice(int num) {
  boolean isgo = false;
  
    for ( int i=0; i<10; i++ ) {
      if ( digitalRead(sunSwitchPin[num]) == HIGH ) {
        isgo = true;
        break;
      }
      delay(20);
    }
  
  while ( isgo ) {
      digitalWrite(sunMotorInA[num], LOW);
      digitalWrite(sunMotorInB[num], HIGH);
      
      isgo = false;
      for ( int i=0; i<10; i++ ) {
        if ( digitalRead(sunSwitchPin[num]) == HIGH ) {
          isgo = true;
          break;
        }
        delay(20);
      }
  }
  digitalWrite(sunMotorInA[num], HIGH);
  digitalWrite(sunMotorInB[num], HIGH);
}

void runPowerMotor(int cmdNum, int cmdValue) {
  //int value = int(0.18f*cmdValue - 71.3f);
  int value = cmdValue / 4;
  Serial.println(value);
  for ( int i=0; i<6; i++ ) {
    analogWrite(powerMotorEn[i], value);
  }
  
  if ( cmdNum == 30 ) {
    for ( int i=0; i<6; i++ ) {
      digitalWrite(powerMotorInA[i], LOW);
      digitalWrite(powerMotorInB[i], LOW);
      digitalWrite(powerMotorEn[i], LOW);
    }
  } else if ( cmdNum == 31 ){
    for ( int i=0; i<6; i++ ) {
      digitalWrite(powerMotorInA[i], HIGH);
      digitalWrite(powerMotorInB[i], LOW);
    }
  }else if ( cmdNum == 32 ) {
    for ( int i=0; i<6; i++ ) {
      digitalWrite(powerMotorInA[i], LOW);
      digitalWrite(powerMotorInB[i], HIGH);
    }
  }
}

void runLight(int cmdValue) {
        switch (cmdValue) {
          case 0 :
            changeLight(cmdValue);
            break;
          case 1 : //Serial.println('1');
            changeLight(cmdValue);
            break;
          case 2 : //Serial.println('2');
            changeLight(cmdValue);
            break;
        }
}

void changeLight(int num) {
  if ( (millis() - lightTimeStamp[num]) >200 ) {
    if (lightStatus[num]==HIGH) {
      lightStatus[num]=LOW;
    } else {
      lightStatus[num]=HIGH;
    }
    digitalWrite(lightPin[num], lightStatus[num]);
    lightTimeStamp[num] = millis();
  }
}

void initServo() {
  for (int i=0;i<4;i++) {
    pwm.setPWM(i, 0, servoValuesInit[i] );
  }
}

boolean receiveCMD() {
  if ( Serial.available()>0 ) {
    int a = (char)(Serial.read());
    int b = (unsigned char)(Serial.read())*256 + (unsigned char)(Serial.read());
    
    int aa = (char)(Serial.read());
    int bb = (unsigned char)(Serial.read())*256 + (unsigned char)(Serial.read());
    if ( (a == aa) && ( b==bb ) ) {
      cmdNum = a;
      cmdValue = b;
      return true;
    } else {
      Serial.println("failt!");
      return false;
    }
  } else
    return false;
}
